#include "comm_app_node.h"

#define REMOTION_IP "127.0.0.1"
#define PORT 8899
CommApp::CommApp(ros::NodeHandle& nh)
{
    nh_ = nh;
    server_ = std::unique_ptr<TcpSerVer>(new TcpSerVer());
    control_cmd_pub_ = nh_.advertise<std_msgs::UInt8>("/devices/control_cmd", 1);
    view_angle_pub_ = nh_.advertise<std_msgs::UInt8>("/view/angle_cmd", 1);
    motor_limit_status_sub_ = nh_.subscribe("/devices/cur_limit_status", 1, &CommApp::CallbackLimitStatus, this);
    motor_working_status_sub_ = nh_.subscribe("/devices/cur_working_status", 1, &CommApp::CallbackWorkingStatus, this);
    motor_limit_status_ = 0;
    motor_working_status_ = 0;
}

CommApp::~CommApp()
{
    Stop();
}

bool CommApp::Init()
{   
    
    server_->RegCallback([](void* param, DataPacket& data){
        //todo
        CommApp* app = (CommApp*)param;
        if(strncmp((char *)data.data,"up",data.len-1) == 0){
            printf("up\n");
            app->LidarControlCmd(app->OPEN_CMD);
        }else if(0 == strncmp((const char *)data.data,"down",data.len-1)){
            printf("down\n");
            app->LidarControlCmd(app->CLOSE_CMD);
        }else if(0 == strncmp((const char *)data.data,"switch", data.len-1)){
            printf("switch\n");
            app->ViewAngleSwitch(0);
        }
    },this);
    std::string ip;
    int port;
    nh_.param<std::string>("remote_ip", ip, REMOTION_IP);
    nh_.param<int>("remote_ip", port, PORT);
    return server_->Init(ip, port);
}

bool CommApp::Start()
{
    server_->Start();
    pub_timer_ = nh_.createTimer(ros::Duration(1), &CommApp::CallbackPubTimer, this);
    return true;
}

bool CommApp::Stop()
{
    if(server_){
        server_->Stop();
        server_ = nullptr;
    }

    return true;
}

void CommApp::CallbackLimitStatus(const std_msgs::UInt8::ConstPtr &msg)
{
    if(msg){
        motor_limit_status_ = msg->data;
    }
}

void CommApp::CallbackWorkingStatus(const std_msgs::UInt8::ConstPtr &msg)
{
    if(msg){
        motor_working_status_ = msg->data;
    }
}

void CommApp::CallbackPubTimer(const ros::TimerEvent &event)
{
    
    uint8_t* data = new uint8_t[2];
    data[0]= motor_limit_status_;
    data[1] = motor_working_status_;
    for(auto iter = server_->getClients().begin(); iter != server_->getClients().end(); ++iter){
        server_->SendData(data, 2, iter->first);
    }
    
}

void CommApp::LidarControlCmd(uint8_t cmd)
{
    std_msgs::UInt8 msg;
    msg.data = cmd;
    control_cmd_pub_.publish(msg);
}

void CommApp::ViewAngleSwitch(uint8_t cmd)
{
    std_msgs::UInt8 msg;
    msg.data = cmd;
    view_angle_pub_.publish(msg);
}